import serial  # 导入模块
from serial import Serial

LOBOT_SERVO_FRAME_HEADER = 0x55
LOBOT_SERVO_MOVE_TIME_WRITE = 1
LOBOT_SERVO_MOVE_TIME_READ = 2
LOBOT_SERVO_MOVE_TIME_WAIT_WRITE = 7
LOBOT_SERVO_MOVE_TIME_WAIT_READ = 8
LOBOT_SERVO_MOVE_START = 11
LOBOT_SERVO_MOVE_STOP = 12
LOBOT_SERVO_ID_WRITE = 13
LOBOT_SERVO_ID_READ = 14
LOBOT_SERVO_ANGLE_OFFSET_ADJUST = 17
LOBOT_SERVO_ANGLE_OFFSET_WRITE = 18
LOBOT_SERVO_ANGLE_OFFSET_READ = 19
LOBOT_SERVO_ANGLE_LIMIT_WRITE = 20
LOBOT_SERVO_ANGLE_LIMIT_READ = 21
LOBOT_SERVO_VIN_LIMIT_WRITE = 22
LOBOT_SERVO_VIN_LIMIT_READ = 23
LOBOT_SERVO_TEMP_MAX_LIMIT_WRITE = 24
LOBOT_SERVO_TEMP_MAX_LIMIT_READ = 25
LOBOT_SERVO_TEMP_READ = 26
LOBOT_SERVO_VIN_READ = 27
LOBOT_SERVO_POS_READ = 28
LOBOT_SERVO_OR_MOTOR_MODE_WRITE = 29
LOBOT_SERVO_OR_MOTOR_MODE_READ = 30
LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE = 31
LOBOT_SERVO_LOAD_OR_UNLOAD_READ = 32
LOBOT_SERVO_LED_CTRL_WRITE = 33
LOBOT_SERVO_LED_CTRL_READ = 34
LOBOT_SERVO_LED_ERROR_WRITE = 35
LOBOT_SERVO_LED_ERROR_READ = 36


def GET_LOW_BYTE(x):
    return x & 0xff


def GET_HIGH_BYTE(x):
    return (x >> 8) & 0xff


def BYTE_TO_HW(A: int, B: int):
    return (A << 8) | (B & 0xff)


def LobotCheckSum(buf):
    sum = 0
    for i in range(2, buf[3] + 2):
        # print(i)
        sum = GET_LOW_BYTE(buf[i] + sum)

    return GET_LOW_BYTE(~sum)


def LobotSerialServoReceiveHandle(SerialX: Serial, ret):
    frameStarted = False
    receiveFinished = False
    frameCount = 0
    dataCount = 0
    dataLength = 2
    recvBuf = [0] * 32

    while SerialX.in_waiting:
        rxBuf = SerialX.read()
        # print(ord(rxBuf))
        # delayMicroseconds(100);
        if not frameStarted:
            # print("not frameStarted")
            if ord(rxBuf) == LOBOT_SERVO_FRAME_HEADER:
                frameCount = frameCount + 1
                if (frameCount == 2):
                    frameCount = 0
                    frameStarted = True
                    dataCount = 1
            else:
                frameStarted = False
                dataCount = 0
                frameCount = 0

        if frameStarted:
            # print("rxBuf = {}".format(frameStarted))
            recvBuf[dataCount] = ord(rxBuf)
            if (dataCount == 3):
                dataLength = recvBuf[dataCount]
                if (int(dataLength) < 3 or dataCount > 7):
                    dataLength = 2
                    frameStarted = False

            dataCount = dataCount + 1
            if dataCount == dataLength + 3:
                if LobotCheckSum(recvBuf) == recvBuf[dataCount - 1]:
                    frameStarted = False
                    for i in range(dataLength):
                        ret[i] = recvBuf[4 + i]
                    # ret = recvBuf[4:4+dataLength]
                    # memcpy(ret, recvBuf + 4, dataLength);
                    return 1

                return -1


def LobotSerialServoMove(SerialX, id, position, time):
    buf = [0] * 10
    if position < 0:
        position = 0
    if position > 1000:
        position = 1000

    buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
    buf[2] = id
    buf[3] = 7
    buf[4] = LOBOT_SERVO_MOVE_TIME_WRITE
    buf[5] = GET_LOW_BYTE(position)
    buf[6] = GET_HIGH_BYTE(position)
    buf[7] = GET_LOW_BYTE(time)
    buf[8] = GET_HIGH_BYTE(time)
    buf[9] = LobotCheckSum(buf)
    SerialX.write(buf)


def LobotSerialServoReadPosition(SerialX: Serial, id):
    count = 10000
    ret = 0
    buf = [0] * 6

    buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
    buf[2] = id
    buf[3] = 3
    buf[4] = LOBOT_SERVO_POS_READ
    buf[5] = LobotCheckSum(buf)

    # ifdef LOBOT_DEBUG
    #   Serial.println("LOBOT SERVO Pos READ");
    #   int debug_value_i = 0;
    #   for (debug_value_i = 0; debug_value_i < buf[3] + 3; debug_value_i++)
    #   {
    #     Serial.print(buf[debug_value_i], HEX);
    #     Serial.print(":");
    #   }
    #   Serial.println(" ");
    # #endif
    # print(SerialX.in_waiting)
    while SerialX.in_waiting > 0:
        SerialX.read()

    SerialX.write(buf)

    while not SerialX.in_waiting:
        count -= 1
        if count < 0:
            return -2048

    RET = LobotSerialServoReceiveHandle(SerialX, buf)
    if RET > 0:
        ret = BYTE_TO_HW(buf[2], buf[1])
    else:
        ret = -2048
    return ret
